#include "read_data.h"

static Eigen::Matrix3d intrinsic_;

read_data_txt::read_data_txt(const std::string& path) {
    path_ = path;
}

std::vector<std::string> read_data_txt::split(std::string str, std::string pattern) {
	std::string::size_type pos;
	std::vector<std::string> result;
	str += pattern;
	int size = str.size();

	for (int i = 0; i < size; i++) {
		pos = str.find(pattern, i);

		if (pos < size) {
			std::string s = str.substr(i, pos - i);
			result.push_back(s);
			i = pos + pattern.size() - 1;
		}
	}
	return result;
}

ImuData read_file_imu_data() {
    static float timestamp = 0.0f;
    ImuData data;
    data.timestamp = timestamp;
    data.ax = static_cast<float>(rand() % 100) / 10.0f - 5.0f;
    data.ay = static_cast<float>(rand() % 100) / 10.0f - 5.0f;
    data.az = static_cast<float>(rand() % 100) / 10.0f - 5.0f;
    data.gx = static_cast<float>(rand() % 100) / 10.0f - 5.0f;
    data.gy = static_cast<float>(rand() % 100) / 10.0f - 5.0f;
    data.gz = static_cast<float>(rand() % 100) / 10.0f - 5.0f;
    timestamp += 0.01f;
    return data;
}

WheelData read_file_wheel_data() {
    static uint64_t timestamp = 0;
    WheelData data;
    data.timestamp = timestamp;
    data.x = static_cast<float>(rand() % 1000) / 10.0f;
    data.y = static_cast<float>(rand() % 1000) / 10.0f;
    data.t = static_cast<float>(rand() % 360);
    data.shift_pos = rand() % 5;
    data.vehicle_speed = static_cast<float>(rand() % 200);
    data.left_front_wheel_pulse = rand() % 1000;
    data.right_front_wheel_pulse = rand() % 1000;
    data.left_rear_wheel_pulse = rand() % 1000;
    data.right_rear_wheel_pulse = rand() % 1000;
    data.turn_radius = static_cast<float>(rand() % 100);
    data.steering_angle = static_cast<float>(rand() % 90);
    timestamp += 10000;
    return data;
}


void read_data_txt::read_sensor_data(std::vector<ImuData>& imu_data_list, std::vector<WheelData>& wheel_data_list) {
    std::ifstream imu_read_file(path_+"imu_data.bin", std::ios::binary);
    std::ifstream wheel_read_file(path_+"wheel_data.bin", std::ios::binary);
    
    if (!imu_read_file.is_open() || !wheel_read_file.is_open()) {
        std::cerr << "无法打开文件进行读取!" << std::endl;
        return;
    }

    // 读取IMU数据
    ImuData imu_temp;
    while (imu_read_file.read(reinterpret_cast<char*>(&imu_temp), sizeof(ImuData))) {
        // 根据imu实际安装位置调整，需要加负号
        // ImuData imu_data = imu_temp;
        // imu_temp.ax = imu_data.ay;
        // imu_temp.ay = imu_data.ax;

        imu_temp.gz = -imu_temp.gz;
        imu_data_list.push_back(imu_temp);
    }
    
    // 读取Wheel数据
    WheelData wheel_temp;
    while (wheel_read_file.read(reinterpret_cast<char*>(&wheel_temp), sizeof(WheelData))) {
        wheel_data_list.push_back(wheel_temp);
    }
    
    // 关闭文件
    imu_read_file.close();
    wheel_read_file.close();
    
    // 显示读取的数据
    std::cout << "读取到 " << imu_data_list.size() << " 条IMU数据:" << std::endl;
    for (size_t i = 0; i < std::min(imu_data_list.size(), static_cast<size_t>(5)); i++) {
        const auto& data = imu_data_list[i];
        std::cout << "IMU[" << i << "]: t=" << data.timestamp 
                  << ", ax=" << data.ax << ", ay=" << data.ay << ", az=" << data.az
                  << ", gx=" << data.gx << ", gy=" << data.gy << ", gz=" << data.gz << std::endl;
    }
    if (imu_data_list.size() > 5) {
        std::cout << "... 省略 " << imu_data_list.size() - 5 << " 条记录" << std::endl;
    }
    
    std::cout << "\n读取到 " << wheel_data_list.size() << " 条Wheel数据:" << std::endl;
    for (size_t i = 0; i < std::min(wheel_data_list.size(), static_cast<size_t>(5)); i++) {
        const auto& data = wheel_data_list[i];
        std::cout << "Wheel[" << i << "]: t=" << data.timestamp 
                  << ", x=" << data.x << ", y=" << data.y << ", theta=" << data.t
                  << ", speed=" << data.vehicle_speed << std::endl;
    }
    if (wheel_data_list.size() > 5) {
        std::cout << "... 省略 " << wheel_data_list.size() - 5 << " 条记录" << std::endl;
    }
}

OriginFeaturePoint::OriginFeaturePoint(int _x, int _y, float _score) : 
    x(_x), y(_y), score(_score) {
    for (int i = 0; i < 128; ++i) {
        descriptor_data[i] = 0.0f;
    }
}

std::vector<FeatureOutput> LoadFrameData(std::vector<FeatureOutput>& results, 
        std::vector<cv::KeyPoint>& undistorted_kps,
        const std::string& filename) {

    std::ifstream infile(filename);
    if (!infile.is_open()) {
        std::cerr << "Error: Could not open feature file." << std::endl;
        return {};
    }


    FeatureOutput current_output;

    // 读取文件头
    std::string header;
    std::getline(infile, header);
    if (header.substr(0, 3) != "===") {
        std::cerr << "Error: Invalid file format." << std::endl;
        return {};
    }

    while (!infile.eof()) {
        std::string line;
        std::getline(infile, line);
        if (line.empty()) continue;

        // LOG(INFO) << "line : " << line << std::endl;

        if (line.find("FeatureOutput:") == 0) {

            // LOG(INFO) << "line : " << line << std::endl;
            // LOG(INFO) << "current_output.fp_vector : " << current_output.fp_vector.size() << std::endl;
            continue;
            // 开始新特征记录
            // if (!current_output.fp_vector.empty()) {
            //     results.push_back(current_output);
            //     current_output.fp_vector.clear();
            // }
            
            // std::stringstream ss(line);
            // ss >> line; // 跳过索引
        } else if (line.find("Timestamp:") == 0) {
            // 读取时间戳
            // std::istringstream ss(line);
            // ss >> current_output.timestamp.sec >> current_output.timestamp.nsec;
            current_output.timestamp.sec = 0;
            current_output.timestamp.nsec = 0;

            // LOG(INFO) << "line : " << line << std::endl;

            // LOG(INFO) << "current_output.timestamp : " << current_output.timestamp.sec << ", " << current_output.timestamp.nsec << std::endl;

        } else if (line.find("Global Feature") == 0) {
            // 读取全局特征
            std::string feature_line;
            std::getline(infile, feature_line);
            std::istringstream iss(feature_line);
            for (int i = 0; i < 256; ++i) {
                iss >> current_output.global_feature[i];
                // LOG(INFO) << "current_output.global_feature[i] : " << current_output.global_feature[i] << std::endl;
            }

            // LOG(INFO) << "line : " << line << std::endl;

        } else if (line.find("Feature Points") == 0) {
            // 读取特征点数量
            int count;
            infile >> count;
            // current_output.fp_vector.resize(count);

            // 读取特征点数据
            for (int i = 0; i < count; ++i) {
                int x, y;
                float score;
                infile >> x >> y >> score;
                // current_output.fp_vector[i].x = x;
                // current_output.fp_vector[i].y = y;
                // current_output.fp_vector[i].score = score;
                current_output.fp_vector.push_back(OriginFeaturePoint(x,y,score));
                
                // LOG(INFO) << "point : " << x << ", " << y << ", " << score << std::endl;
            }
        } else if (line.find("local descriptor") == 0) {
            int count;
            infile >> count;

            for (int i = 0; i < count; ++i) {
                std::string feature_line;
                std::getline(infile, feature_line);
                std::istringstream iss(feature_line);
                for (int j = 0; j < 128; ++j) {
                    iss >> current_output.fp_vector[i].descriptor_data[j];
                    // LOG(INFO) << "current_output.fp_vector[i].descriptor_data[j] : " << current_output.fp_vector[i].descriptor_data[j] << std::endl;
                }
            }
        } else if (line.find("Undistorted KeyPoints") == 0) {
            int count;
            infile >> count;
            undistorted_kps.resize(count);

            for (int i = 0; i < count; ++i) {
                cv::KeyPoint key;
                infile >> key.pt.x >> key.pt.y >> key.response;
                undistorted_kps[i] = key;
            }

            // LOG(INFO) << "count : " << count << std::endl;

            // LOG(INFO) << "line : " << line << std::endl;

        }

        // LOG(INFO) << "line : " << line << std::endl;
    }

    // 添加最后一个结果
    if (!current_output.fp_vector.empty()) {
        results.push_back(current_output);
    }

    // std::string str_line;
    // std::getline(infile, str_line);

    // LOG(INFO) << "str_line : " << str_line << std::endl;

    // int count;
    // infile >> count;
    // if(count > 500) {
    //     // LOG(FATAL) << " feature number is wrong : " << count << std::endl;
    //     return results;
    // }
    // undistorted_kps.resize(count);
    
    // LOG(INFO) << "count : " << count << std::endl;

    // for (int i = 0; i < count; ++i) {
    //     cv::KeyPoint key;

    //     infile >> key.pt.x >> key.pt.y >> key.response;
    //     undistorted_kps[i] = key;

    //     // float x, y, response;
    //     // undistorted_kps[i].pt.x = key.pt.x;
    //     // undistorted_kps[i].pt.y = key.pt.y;
    //     // undistorted_kps[i].response = key.response;

    //     LOG(INFO) << "undistorted_kps[i].pt.x : " << undistorted_kps[i].pt.x << std::endl;
    // }

    infile.close();

    // LOG(INFO) << "undistorted_kps : " << undistorted_kps.size() << std::endl;

    return results;
}

// 转换函数
std::vector<feature::FeaturePoint> ConvertFeatureOutputToFeaturePoints(
    const std::vector<FeatureOutput>& results, 
    const std::vector<cv::KeyPoint>& undistorted_kps) {
    
    std::vector<FeaturePoint> feature_points;

    // double fx = 448.13773;
    // double fy = 252.07747;
    // double cx = 640;
    // double cy = 360;
    // double image_width_scale_ratio = 2.0;
    // double image_height_scale_ratio = 1.5;
    
    // LOG(INFO) << " intrinsic_ ===== :\n " << intrinsic_ << std::endl;

    double fx = intrinsic_(0,0);
    double fy = intrinsic_(1,1);
    double cx = intrinsic_(0,2);
    double cy = intrinsic_(1,2);
    double image_width_scale_ratio = 2.0;
    double image_height_scale_ratio = 1.5;

    for (size_t i = 0; i < results.size(); ++i) {
        const FeatureOutput& output = results[i];
        
        for (size_t j = 0; j < output.fp_vector.size(); ++j) {
            const OriginFeaturePoint& origin_fp = output.fp_vector[j];
            
            feature::FeaturePoint new_fp;
            
            // 转换图像坐标
            new_fp.image_point << origin_fp.x*image_width_scale_ratio, origin_fp.y*image_height_scale_ratio;
           
            // LOG(INFO) << " image_point ===== : " << new_fp.image_point.transpose() << std::endl;

            // 转换描述符
            for (int k = 0; k < 128; ++k) {
                new_fp.descriptor(k) = origin_fp.descriptor_data[k];

                // LOG(INFO) << " origin_fp.descriptor_data[k] ===== : " << origin_fp.descriptor_data[k] << std::endl;
            }
            // LOG(INFO) << " new_fp.descriptor ===== : " << new_fp.descriptor.transpose() << std::endl;

            // 设置分数
            new_fp.score = origin_fp.score;
            
            // 转换全局特征
            for (int k = 0; k < 256; ++k) {
                new_fp.global_feature(k) = output.global_feature[k];
            }

            cv::KeyPoint undistorted_kp = undistorted_kps[j];
      
            // new_fp.undistorted_point << undistorted_kp.pt.x/image_width_scale_ratio, undistorted_kp.pt.y/image_height_scale_ratio;
            // new_fp.normalized_point << (undistorted_kp.pt.x/image_width_scale_ratio-cx)/fx, (undistorted_kp.pt.y/image_height_scale_ratio-cy)/fy, 1.0;
            
            new_fp.undistorted_point << undistorted_kp.pt.x, undistorted_kp.pt.y;
            new_fp.normalized_point << (undistorted_kp.pt.x-cx)/fx, (undistorted_kp.pt.y-cy)/fy, 1.0;

            // Vector2d predict_undistorted_point;
            // predict_undistorted_point << fx*new_fp.normalized_point[0]+cx, fy*new_fp.normalized_point[1]+cy;

            // LOG(INFO) << " predict_undistorted_point ===== : " << predict_undistorted_point.transpose() << std::endl;
            // LOG(INFO) << " new_fp.preundistorted_point ===== : " << new_fp.undistorted_point.transpose() << std::endl;

            // LOG(INFO) << " new_fp.normalized_point ===== : " << new_fp.normalized_point.transpose() << std::endl;

            feature_points.push_back(new_fp);
        }
    }

    return feature_points;
}

void read_data_txt::read_feature(std::vector<feature::FeaturePoint>& feature_data) {
    std::vector<FeatureOutput> results; 
    std::vector<cv::KeyPoint> undistorted_kps;

    static int frame_count = -1;

    const std::string filename = path_ + "feature/frame_" + std::to_string(++frame_count) + ".txt";
    LoadFrameData(results, undistorted_kps, filename);
        
    // LOG(INFO) << "results " << results.size() << std::endl;
    // LOG(INFO) << "undistorted_kps " << undistorted_kps.size() << std::endl;
    if(undistorted_kps.empty()) {
        results.clear();
        return;
    }
    feature_data = ConvertFeatureOutputToFeaturePoints(results, undistorted_kps);  
}

Eigen::Matrix4d read_data_txt::read_extrinsic() {
    const std::string filename = path_ + "feature/extrinsic.txt";

   std::ifstream inFile(filename);
    if(!inFile.is_open()) {
        throw std::runtime_error("Cannot open input file");
    }

    std::vector<std::vector<float>> data;
    std::string line;
    
    while(std::getline(inFile, line)) {
        // 移除行尾分号
        if(!line.empty() && line.back() == ';') {
            line.pop_back();
        }
        
        std::vector<float> row;
        std::istringstream iss(line);
        std::string value;
        
        while(std::getline(iss, value, ',')) {
            try {
                row.push_back(std::stod(value));
            } catch(...) {
                throw std::runtime_error("Invalid number format");
            }
        }
        
        if(!row.empty()) {
            data.push_back(row);
        }
    }

    if(data.empty()) {
        throw std::runtime_error("No valid data found");
    }

    Eigen::Matrix4d result;
    for(int i = 0; i < 4; ++i) {
        for(int j = 0; j < 4; ++j) {
            result(i,j) = data[i][j];
        }
    }
    
    return result;
}

Eigen::Matrix3d read_data_txt::read_intrinsic() {
    double fx = 448.13773;
    double fy = 252.07747;
    double cx = 640;
    double cy = 360;
    // double image_width_scale_ratio = 2.0;
    // double image_height_scale_ratio = 1.5;
    double image_width_scale_ratio = 1.0;
    double image_height_scale_ratio = 1.0;
    Eigen::Matrix3d intrinsic = Eigen::Matrix3d::Identity();
    intrinsic(0,0) = fx/image_width_scale_ratio;
    intrinsic(1,1) = fy/image_height_scale_ratio;
    intrinsic(0,2) = cx/image_width_scale_ratio;
    intrinsic(1,2) = cy/image_height_scale_ratio;
    intrinsic_ = intrinsic;

    return intrinsic;
}